//
// Created by LinorInk on 2024/5/13.
//
#include "user.h"
#include "task.h"
#include "servo_inf.h"
#include "actions.h"
#include "UART.h"
#include "utils.h"

taskTypedef action_controller_task_typedef;

void action_controller_task();

void action_controller_init() {
    Task_regist(&action_controller_task_typedef, 0, 0xff, action_controller_task);
}

void action_controller_set_action(action_controller_t *action, float scale) {
    uint8_t last_index = (action->index + action->count - 1) % action->count;
    UART_Write(&action->index, 1);

    if (scale > 0) {
        if (getDTime(action->last_update_tick) * scale < action->tick[last_index]) return;

        if (action->index < action->count) {
            servo_setAction(action->action[action->index]);
            action->index++;
        } else {
            action->index = 0;
        }
    } else {
        if (getDTime(action->last_update_tick) * (-scale) < action->tick[last_index]) return;

        if (action->index > 0) {
            servo_setAction(action->action[action->index]);
            action->index--;
        } else {
            action->index = action->count - 1;
        }
    }

    action->last_update_tick = HAL_GetTick();
}

static action_controller_t *last_action = NULL;

void action_controller_task() {
    switch (DR16_COMMAND) {
        case IDLE:
            servo_setAction(action_idle);   // 清零
            if(last_action != NULL) {
                last_action->index = 0;
                last_action = NULL;
            }
            break;
        case CHEER:
            last_action = &action_cheer_controller;
            action_controller_set_action(&action_cheer_controller, 1);
            break;
        case IDLE_HIGH:
            servo_setAction(action_idle_high);
            break;
        case IDLE_SHORT:
            servo_setAction(action_idle_short);
            break;
        case IDLE_DOWN:
            servo_setAction(action_idle_down);
            break;
        case MOVE_FORWARD:
            last_action = &action_walk_controller;
            action_controller_set_action(&action_walk_controller, 1);
            break;
        case MOVE_BACKWARD:
            last_action = &action_back_controller;
            action_controller_set_action(&action_back_controller, 1);
            break;
        case MOVE_LEFT:
            last_action = &action_move_left_controller;
            action_controller_set_action(&action_move_left_controller, 1);
            break;
        case MOVE_RIGHT:
            last_action = &action_move_right_controller;
            action_controller_set_action(&action_move_right_controller, 1);
            break;
        case TURN_ROUND_LEFT:
            last_action = &action_turn_round_left_controller;
            action_controller_set_action(&action_turn_round_left_controller, 1);
            break;
        case TURN_ROUND_RIGHT:
            last_action = &action_turn_round_right_controller;
            action_controller_set_action(&action_turn_round_right_controller, 1);
            break;
        case TURN_ROUND_LEFT_HIGH:
            last_action = &action_turn_round_left_high_controller;
            action_controller_set_action(&action_turn_round_left_high_controller, 1);
            break;
        case TURN_ROUND_RIGHT_HIGH:
            last_action = &action_turn_round_right_high_controller;
            action_controller_set_action(&action_turn_round_right_high_controller, 1);
            break;
        case CHEER_1:
            last_action = &action_cheer_1_controller;
            action_controller_set_action(&action_cheer_1_controller, 1);
            break;
    }
    }
